;; should be able to run your STRIPS by typing:
;; (strips (list start-state goal-state operators))
 
; start state
(setf start-state
  '((at home)
   (has store beer)
   (has me car)))
 
; goal state
(setf goal-state
  '((has me beer)(at home)))
 
; operators
(setf operators
 
      ;ACTION
  '(((buy X) 
   ;PRECOND
   ((at store) (has store X))
   ;ADD
   ((has me X))
   ;DELETE
   ())
    
  ((goto X)
     ((has me car))
     ((at X))
     ()))
)




; call the algorithm with the input
(defun strips (input)
    (let ((start-state (first input))
          (goal-state (second input)) 
          (operators (third input)))
        (strips-alg start-state goal-state operators '())
    )
)
 
; see if the top node is satisfied in the knowledge base
(defun top-node-satisfied (node kb)
    (cond
        ((null kb) nil)
        ; return true if this node is in the knowledge base
        ((equal node (first kb)) t)
        (t (top-node-satisfied node (rest kb)))
    )
)
 
; check if the node is an operator
(defun is-operator (node)
    (not (atom (first node)))
)
 
; returns true if this is not a goal
(defun is-goal (node)
    (not (is-operator node))
)
 
; does a recursive search and replace on a list
(defun match-list (sub match lst new-lst)
    (cond
        ((null lst) new-lst)
        (t
            (match-list
                sub
                match
                (rest lst)
                (append new-lst (list (substitute sub match (first lst)))))
        )
    )
)
 
; binds the substitutions with a lst
(defun bind-substitutions (sub-list lst)
    (cond
        ((null sub-list) lst)
        (t
            (let* ((sub-var (rest (first sub-list)))
                   (match-var (first (first sub-list))))
                (bind-substitutions
                    (rest sub-list)
                    (match-list sub-var match-var lst '()))
            )
        )
    )
)
 
; deletes matches from the kb
(defun operator-delete-kb (kb delete-list)
    (cond
        ((null delete-list) kb)
        (t (operator-delete-kb
            (delete-from-kb
                kb
                (first delete-list)
                '()) (rest delete-list)))
    )
)
 
; delete one item from the kb
(defun delete-from-kb (kb item save-kb)
    (cond
        ((null kb) save-kb)
        ((equal (first kb) item) (delete-from-kb (rest kb) item save-kb))
        (t (delete-from-kb (rest kb) item (append save-kb (list (first kb)))))
    )
)
 
; utility functions for common operations
(defun operator-from-operator (operator)
    (first operator)
)
 
(defun pre-from-operator (operator)
    (second operator)
)
 
(defun add-from-operator (operator)
    (third operator)
)
 
(defun del-from-operator (operator)
    (fourth operator)
)
 
; try and unify the add list with the top node and return a list
; of the operators which unify
(defun find-matches (operators top-node matches)
    (cond
        ((null operators) matches)
        (t
            (let* ((operator (first operators))
                   (add-list (add-from-operator operator))
                   (sub-list (unify-all add-list (list top-node) '())))
                (cond
                    ((equal sub-list 'fail)
                        (find-matches (rest operators) top-node matches)
                    )
                    (t
                        (let ((bind-op (unify-operator sub-list operator)))
                            (find-matches
                                (rest operators)
                                top-node
                                (append
                                    matches
                                    (list bind-op)))
 
                        )
                    )
                )
            )
        )
    )
)
 
; bind the subsitution list with the operator
(defun unify-operator (sub-list operator)
    (cond
        ((null sub-list) operator)
        (t
            (let* ((sub-var (rest (first sub-list)))
                   (match-var (first (first sub-list)))
                   (op-un (substitute
                        sub-var match-var (operator-from-operator operator)))
                   (pre-un (match-list
                        sub-var match-var (pre-from-operator operator) '()))
                   (add-un (match-list
                        sub-var match-var (add-from-operator operator) '()))
                   (del-un (match-list
                        sub-var match-var (del-from-operator operator) '()))
                   (new-operator (list op-un pre-un add-un del-un)))
                (unify-operator (rest sub-list) new-operator)
            )
        )
    )
)
 
; recurse over the operators list checking for failure
(defun apply-matches (kb stack operators plan matches)
    (cond
        ((null matches) 'fail)
        (t
            (let* ((match (first matches))
                   (pre-list (pre-from-operator match))
                   (sub-list (unify-all kb pre-list '()))
                   (pc-satisfied (not (equal sub-list 'fail)))
                   ; call strips alg with the new 
                   (strips-result 
                        (cond
                            ((equal sub-list 'fail) 
                                ; if unify failed, we need to add PC and op
                                (let* ((new-stack (append
                                                        (reverse pre-list)
                                                        (list match)
                                                        stack)))
                                    (strips-alg kb new-stack operators plan)
                                )
                            )
                            (t
                                ; if unify passed, we need to bind
                                (let ((new-operator 
                                        (unify-operator sub-list match)))
                                    (strips-alg
                                        kb
                                        (append (list new-operator) stack)
                                        operators
                                        plan)
                                )
                            )
                        )
                    ))
                (cond
                    ; backtrack and try the next match
                    ((equal strips-result 'fail)
                        (apply-matches
                            kb
                            stack
                            operators
                            plan
                            (rest matches)))
                    ; return the final result
                    (t strips-result)
                )
            )
        )
    )
)
 
; recursively call the strips alg to reach the plan
(defun strips-alg (kb stack operators plan)
    (let ((top-node (first stack))
          (new-stack (rest stack)))
        (cond
            ((null stack) plan)
            ((is-goal top-node)
                (cond
                    ((top-node-satisfied top-node kb)
                        (strips-alg kb new-stack operators plan)
                    )
                    (t
                        (let* ((matches (find-matches
                            operators top-node '())))
                            (apply-matches kb stack operators plan matches)
                        )
                    )
                )
            )
            ; we know this is redundant, but this is more readable
            ((is-operator top-node)
                (let* ((delete-list (del-from-operator top-node))
                       (add-list (add-from-operator top-node))
                       (new-plan (append 
                           plan
                           (list (operator-from-operator top-node))))
                       (new-kb (append
                           (operator-delete-kb kb delete-list) add-list)))
                    (strips-alg new-kb new-stack operators new-plan)
                )
            )
        )
    )
)
 
 
(defun unify-all (kb pcl s-list)
    (cond
        ; this is our base case. return the subsitution list
        ((null pcl) s-list)
        (t
            ; otherwise recurse over the precond list to find the
            ; full subsitution
            (let ((sub-list (unify-first-pc kb kb pcl s-list)))
                (cond
                    ((equal sub-list 'fail) 'fail)
                    (t
                        (unify-all kb (rest pcl) sub-list)
                    )
                )
            )
        )
    )   
)
 
 
(setf test-pcl '((has store beer) (at home)))
(setf test-kb '((has store beer) (at home) (has me car)))
 
(setf pcl '((clear X) (on X blockC)))
 
(setf good-kb '((clear blockA) (clear blockB) (on blockA table)
        (on blockB blockC) (on blockC table)))
 
(setf bad-kb '((clear blockA) (clear blockD) (on blockA table)
           (on blockD blockB) (on blockB blockC) (on blockC table)))
 
(defun unify-first-pc (kb saved-kb pcl s-list)
  (cond ((null kb) 'fail)
    ((null pcl) s-list)
 
    ;; if our arguments are valid, try unifying the first
    ;; precondition with the knowledge base. Then check
    ;; the substitution we create (if any) against the rest
    ;; of the preconditions on the precondition list
     
    (t (let ((subst (unify (car kb) (car pcl) s-list)))
 
         (cond
 
          ;; this recurses down the knowledge base trying
          ;; to unify the first precondition
          ((eq subst 'fail) (unify-first-pc (cdr kb)
                             saved-kb
                             pcl
                             s-list))
 
          ;; if we get back null, it means we unified without
          ;; having to substitute anything (there were no variables)
          ;; check to see if we can unify the rest of the knowledge
          ;; base against the world.
          ((null subst) (check-all-pc saved-kb (cdr pcl) subst))
 
          ;; if, when we checked the rest of the preconditions
          ;; using our current substitution, we fail, then we
          ;; need to backtrack and try a new substitution for
          ;; our first precondition.
          ((eq (check-all-pc saved-kb (cdr pcl) subst) 'fail)
           (unify-first-pc (cdr kb)
                   saved-kb
                   pcl
                   s-list))
 
          ;; finally, we found a substitution that worked for
          ;; the first precondition and didn't cause any problems
          ;; with the rest of them.
          (t subst)))
       )
    )
  )
 
 
(defun check-all-pc (kb pcl s-list)
  (cond ((null pcl) s-list)
    ((eq (check-one-pc kb (car pcl) s-list) 'fail) 'fail)
    (t (check-all-pc kb (cdr pcl) s-list))))
 
(defun check-one-pc (kb pc s-list)
  (cond ((null kb) 'fail)
    ((eq (unify (car kb) pc s-list) 'fail)
     (check-one-pc (cdr kb) pc s-list))
    (t t)))
 
(defun unify (x y subst)
  (cond ((equal x y) subst)
 
    ((equal subst 'fail) 'fail)
 
    ((var? x) (unify-variable x y subst))
 
    ((var? y) (unify-variable y x subst))
 
    ((or (atom x) (atom y)) 'fail)
 
    (t (unify (rest x) (rest y)
          (unify (first x) (first y) subst)))))
 
 
 
(defun unify-variable (var val subst)
  (cond ((equal var val) subst)
 
    ((bound? var subst)
     (unify (lookup var subst) val subst))
 
    ((and (var? val) (bound? val subst))
     (unify var (lookup val subst) subst))
 
    ((occurs-in? var val subst) 'fail)
 
    (t (extend-subst var val subst))))
 
 
(defun occurs-in? (var x subst)
  (cond ((equal var x) t)
 
    ((bound? x subst)
     (occurs-in? var (lookup x subst) subst))
 
    ((consp x) (or (occurs-in? var (first x) subst)
               (occurs-in? var (rest x) subst)))
 
    (t nil)))
 
(defun var? (x)
  (member x '(A B C D E F G H I J K L M N O P Q R S T U V W X Y Z)))
 
 
(defun bound? (x subst)
  (assoc x subst))
 
 
(defun lookup (var subst)
  (cdr (assoc var subst)))
 
 
(defun extend-subst (var val subst)
  (cons (cons var val) subst))